function [PathPoints, st, ErrCode, info] = SPRPSNPath(vehx, vehy, vehth, tpmx, tpmy, tpmth, PSInfo, VEHInfo, ParamInfo)
% 直行前进 + 右转前进 + 直行后退

% 输入数据处理
ymin = ParamInfo.ymin;
NR = ParamInfo.NR;
VehWidth = VEHInfo.VehWidth;
VehRear  = VEHInfo.VehRear;
VehLength = VEHInfo.VehLength;
cr2x = PSInfo.cr2x;%#ok
% 初始化返回结果 
info = struct('Fcount',uint8(0),'Ncount',uint8(0),'count',uint8(0),'c2dist',single(0),'ymin',single(0));
info.count = uint8(0);
ErrCode = uint8(0);
st = true;
PathPoint  = struct('XPos',single(0),'YPos',single(0),'Headangle',single(0),'Kappa',single(0),'PathLength',single(0),'PathType',Pt_Typ_PathType_en.Initial);
PathPoints = repmat(PathPoint,[1 8]);

% 关键点计算
% 1 求交点Ix, Iy
a1 =  sin(deg2rad(vehth));
b1 = -cos(deg2rad(vehth));
c1 = vehy*cos(deg2rad(vehth)) - vehx*sin(deg2rad(vehth));

a2 =  sin(deg2rad(tpmth));
b2 = -cos(deg2rad(tpmth));
c2 = tpmy*cos(deg2rad(tpmth)) - tpmx*sin(deg2rad(tpmth));

[Ix, Iy, Ist] = P_intersectLines(a1, b1, c1, a2, b2, c2);
% veh和tpm没有交点
if ~Ist
    st = false;
    ErrCode = uint8(8);
    return;
end
% 2 求切点1
dth = abs(OffCPIP_angdiff(deg2rad(vehth),deg2rad(tpmth)));
L = NR*tan(dth/single(2));
T1x  = Ix - L*cos(deg2rad(vehth));
T1y  = Iy - L*sin(deg2rad(vehth));
% 判断切点1是否在veh点在后方
dotval = (T1x - vehx)*cos(deg2rad(vehth)) + (T1y - vehy)*sin(deg2rad(vehth));
% 第一段直线段不存在
if dotval < single(0)   
    st = false;
    ErrCode = uint8(9);
    return;
end
% 3 求切点2
T2x  = Ix + L*cos(deg2rad(tpmth));
T2y  = Iy + L*sin(deg2rad(tpmth));
% 判断NP2的右前点或左前点是否小于ymin
NP2LFy = T2y + (VehLength - VehRear)*sin(deg2rad(tpmth)) + VehWidth/single(2)*sin(deg2rad(tpmth)+single(pi/2));
NP2RFy = T2y + (VehLength - VehRear)*sin(deg2rad(tpmth)) + VehWidth/single(2)*sin(deg2rad(tpmth)-single(pi/2));
% NP2的右前点或左前点小于ymin
if NP2LFy < ymin || NP2RFy < ymin
    st = false;
    ErrCode = uint8(10);
    return;
end

sl = sqrt((T1x-vehx)^2+(T1y-vehy)^2);
% 返回结果更新
PathPoints(1).XPos = vehx;
PathPoints(1).YPos = vehy;
PathPoints(1).Headangle = vehth;
PathPoints(1).PathType = Pt_Typ_PathType_en.FLine;
PathPoints(1).Kappa    = single(0);
PathPoints(1).PathLength = sl;

PathPoints(2).XPos = T1x;
PathPoints(2).YPos = T1y;
PathPoints(2).Headangle = vehth;
PathPoints(2).PathType = Pt_Typ_PathType_en.FRCircle;
PathPoints(2).Kappa    = -single(1/NR);
PathPoints(2).PathLength = dth*NR;

PathPoints(3).XPos = T2x;
PathPoints(3).YPos = T2y;
PathPoints(3).Headangle = tpmth;
PathPoints(3).PathType = Pt_Typ_PathType_en.BLine;
PathPoints(3).Kappa    = single(0);
PathPoints(3).PathLength = sqrt((T2x-tpmx)^2+(T2y-tpmy)^2);

PathPoints(4).XPos = tpmx;
PathPoints(4).YPos = tpmy;
PathPoints(4).Headangle = tpmth;
PathPoints(4).PathType = Pt_Typ_PathType_en.Initial;
PathPoints(4).Kappa    = single(0);
PathPoints(4).PathLength = single(0);
info.count = uint8(3);
p1ok = OffCPIP_CheckKeyPathPoint(PathPoints(1),PathPoints(2));
p2ok = OffCPIP_CheckKeyPathPoint(PathPoints(2),PathPoints(3));
p3ok = OffCPIP_CheckKeyPathPoint(PathPoints(3),PathPoints(4));
if ~(p1ok&&p2ok&&p3ok)
    st = false;
end

% 判断第一段直行的距离是否太短，太短会影响预控制效果。
% 如果第一段直行距离太短，那么应该放大圆的半径来连接。
DisableShortFLine = true;
if sl < single(0.3) && DisableShortFLine
    % 重新计算新大圆的半径
    NNR = NR + sl/tan(dth/single(2));
    L = NNR*tan(dth/single(2));
    % 切点1正好是veh点
    % 求切点2
    T2x  = Ix + L*cos(deg2rad(tpmth));
    T2y  = Iy + L*sin(deg2rad(tpmth));
    % 判断NP2的右前点或左前点是否小于ymin
    NP2LFy = T2y + (VehLength - VehRear)*sin(deg2rad(tpmth)) + VehWidth/single(2)*sin(deg2rad(tpmth)+single(pi/2));
    NP2RFy = T2y + (VehLength - VehRear)*sin(deg2rad(tpmth)) + VehWidth/single(2)*sin(deg2rad(tpmth)-single(pi/2));
    % NP2的右前点或左前点小于ymin
    if NP2LFy < ymin || NP2RFy < ymin
        st = false;
        ErrCode = uint8(10);
        return;
    end
    % 返回结果更新
    PathPoints(1).XPos = vehx;
    PathPoints(1).YPos = vehy;
    PathPoints(1).Headangle = vehth;
    PathPoints(1).PathType = Pt_Typ_PathType_en.FRCircle;
    PathPoints(1).Kappa    = -single(1/NNR);
    PathPoints(1).PathLength = dth*NNR;

    PathPoints(2).XPos = T2x;
    PathPoints(2).YPos = T2y;
    PathPoints(2).Headangle = tpmth;
    PathPoints(2).PathType = Pt_Typ_PathType_en.BLine;
    PathPoints(2).Kappa    = single(0);
    PathPoints(2).PathLength = sqrt((T2x-tpmx)^2+(T2y-tpmy)^2);

    PathPoints(3).XPos = tpmx;
    PathPoints(3).YPos = tpmy;
    PathPoints(3).Headangle = tpmth;
    PathPoints(3).PathType = Pt_Typ_PathType_en.Initial;
    PathPoints(3).Kappa    = single(0);
    PathPoints(3).PathLength = single(0);
    info.count = uint8(2);
    p1ok = OffCPIP_CheckKeyPathPoint(PathPoints(1),PathPoints(2));
    p2ok = OffCPIP_CheckKeyPathPoint(PathPoints(2),PathPoints(3));
    if ~(p1ok&&p2ok)
        st = false;
    end
end
end

    


